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Observer design for position and velocity bias estimation 
from a single direction output 

Florent Le Bras, Tarek Hamel, Robert Mahony, Claude Samson, 


Abstract —This paper addresses the problem of estimating 
the position of an object moving in R" from direction and 
velocity measurements. After addressing observability issues 
associated with this problem, a nonlinear observer is designed 
so as to encompass the case where the measured velocity is 
corrupted by a constant bias. Global exponential convergence 
of the estimation error is proved under a condition of persistent 
excitation upon the direction measurements. Simulation results 
illustrate the performance of the observer. 

I. Introduction 

There is is rich literature in vision based pose estimation 
driven by advances in the structure from motion in the field 
of computer vision [1]. Most of the recent structure from 
motion algorithms are formulated as an optimisation problem 
over a set of selected images [2], however, recent work has 
emphasised the importance of considering motion models 
and filtering techniques [3] for a class of important problems. 
Recursive filtering methods for vision based structure from 
motion and pose estimation themselves have a rich history 
primarily associated with stochastic filter design such as 
EKF, unscented filters and particle filters [4], [5], [6], [7]. A 
comparison of EKF and particle filter algorithms for vision 
based SLAM is available in [8]. Although nonlinear observer 
design does not provide a stochastic interpretation of the state 
estimate they hold the promise to handle the non-linearity of 
the vision pose estimation problem in a robust and natural 
manner [9]. Ghosh and subsequent authors consider non¬ 
linear observers on the class of perspective systems [10], 
[11], [12], [13], [14], that is systems with output in a 
projective space obtained as a quotient of the state space. 
Perspective outputs y{x) are of the form 

y =(—,■■•-, 1 ) 

Tn 

and correspond to the nonlinear projection along rays through 
the origin onto an affine image plane perpendicular to the 
focal axis. The output representation is attractive in that it 
corresponds to the normal representation of vision data for 
perspective cameras. Indeed, there are a number of works 
that consider filtering for directly, rather than estimating 
the camera position [15], [16], [17], corresponding to image 
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tracking. Although significant work has been based on this 
output representation, it tends to lead to complex observer 
and filter design and difficult analysis [10], [11], [12], [13], 
[14]. An additional question of importance concerns the rate 
of convergence of an observer and recent work has addressed 
this question in the context of controlling the camera motion 
to improve observability of the problem and increase the rate 
of convergence of the observer [18]. 

The present paper contributes further to the field of nonlin¬ 
ear observer design for systems with direction outputs. The 
key contribution that we make is the development of an ele¬ 
gant and rigorous stability analysis for a simple filter design. 
The filter is designed for a single bearing measurement and 
relies on the motion of the camera to generate persistence 
of excitation of the innovation in order to guarantee global 
asymptotic convergence. Rather than using the perspective 
outputs favoured in previous papers we use direction outputs 



corresponding to projection onto a virtual spherical image 
plane and differing from perspective outputs only in the 
scaling. The two formulations are essentially equivalent from 
a systems perspective in the region where perspective outputs 
are defined. However, we believe that the direction output 
representation contributes to the simplicity of the observer 
proposed in the present paper. We characterise the rate of 
convergence of the filter in terms of the persistence of 
excitation property. We then consider the case when the 
measurement of velocity of the camera is perturbed by an 
unknown bias. To the authors knowledge, this problem has 
not been considered in the nonlinear observer literature. We 
provide a rigorous proof of the global asymptotic stability 
of the observer state for this case by exploiting a novel 
state transformation. The simulations provided demonstrate 
the performance of the filter. 

The paper is organised along five sections. Following the 
present introduction. Section introduces the system under 
consideration and points out observability properties attached 
to it. Section |m] develops the main results of the paper. 
Section [^present a few illustrative simulations. Concluding 
remarks are provided in Section [V] 



II. Problem description 

The system considered is the kinematics of an object 
moving in K” 

X = V + a (1) 

2/ = ^ e (2) 

where v G M" is the velocity of the object and a G M” 
represents any unknown bias. Let 5'"“^ denote the unit 
sphere, the space of measurements y G R" such thaj^ 
12/1 = 1. An example of such a measurement is the bearing 
in obtained from a camera looking at a moving object. 

In most applications the unknown velocity a G M" 
(with n = 3) represents the velocity of the fluid in which 
evolves the moving object or/and any bias that affects the 
measurement of v. In this paper, for the sake of generality, 
we consider an arbitrary dimension n > 2. We emphasize 
that the value of y G and v G R" must be known at 

all times. 

A. Observability analysis 

We first give a general observability criterion. The fol¬ 
lowing persistency of excitation condition will then yield an 
observability result for system OH- 

Definition 2.1: The direction y G S"' is called persis¬ 
tently exciting if there exist <5 > 0 and 0 < p < S such that 
for all t 

/ > b'l, with TTy = {I - yy^) (3) 

For future use, note that ^ is equivalent to 

pt-\-S 

WbGS^-^ : \ny(^r)b\^dT > p, ( 4 ) 


Definition 2.3: A system is called strongly observable if 
all pairs of distinct initial states are distinguished by all 
admissible inputs. It is called weakly observable if every 
pair of distinct initial states is distinguished by at least one 
admissible input. 

Reasons to differentiate between strong observability and 
weak observability are well explained in the non-linear 
control literature. For complementary details on this subject 
we refer the reader to a classical work by Sussmann [19]. 

Lemma 2.4: The system (fT® complemented with the 

equation a = 0, with ^ ^ the system state vector, 

V as the system input, and y as the system output, is weakly 
observable but not strongly observable. 

Proof: Choose, for instance, the input v{t) = 

(cos(f), sin(f), 0 ... 0)^. The solutions to the system are then 
given by x{f) = a;(0) -I- (sin(t) -f at, — cos(f) -j- at,0 ... 0)^ 
and one easily verifies that yi{t) = 2 / 2 (f), Vf, implies that 
a;i(0) = a;2(0). This establishes the weak observability 
property of the system. Note also that the chosen input 
renders both outputs yi{t) and 2 / 2 (f) persistently exciting 
in the sense of the definition •in). On the other hand, one 
verifies that, if the input v is constant, then initial states 
a;i(0) = fci(r; -I- a) and 312 ( 0 ) = k 2 {v -f a), with ki and ^2 
denoting arbitrary positive numbers, can not be distinguished 
because 2/1 and 2/2 are constant and equal in this case. This 
proves that the system is not strongly observable. ■ 

The weak observability property of the system justifies the 
introduction of the persistence condition evoked previously 
to characterize ’’good” outputs (produced by ’’good” inputs) 
yielding a property of ’’uniform” observability that renders 
the state-observation problem addressed in the next section 
well-posed. 


Another characterization of persistent of excitation, in terms 
of the property that the time-derivative of y must satisfy, is 
pointed out in the following lemma 

Lemma 2.2: Assume that y(f) is uniformly continuous 
and bounded, then relation (|^ (respectively Q) is equivalent 
to: 

>0, 3r e [t, f -f <5] such that | 2 /(r)| > £ (5) 

Proof: The proof of this lemma is given in Appendix ■ 
Note that the uniform continuity and boundedness of y is 
automatically granted when v is itself uniformly continuous 
and bounded and |x| is lower bounded by some positive 
number. 

Recall that two different points Xi,X2 G R" are said 
distinguishable, if there exists an input v{t) G R" and a time 
ti such that for solutions xi(t), X 2 (t) of Q with a;i( 0 ) = x°, 
2 : 2 ( 0 ) = X 2 we have y{xi{ti)) 7 ^ y[x 2 (ti)). Equivalently, in 
this case one says that the admissible input distinguishes the 
two initial states, and also that two initial states of system (jT]- 
1 ^ are indistinguishable if they are not distinguished by any 
admissible input. 

* I. I stands for the Euclidean norm of vectors and 11.11 is the induced 
matrix norm. 


III. Observer design 

The problem of state observation refers to the design of 
an algorithm that allows one to recover actual state values 
from the observation of previous outputs. We start by the 
observer design for the classical situation addressed in the 
literature where the unknown constant velocity bias a is equal 
to zero. The situation when this term is different from zero 
and unknown a priori is addressed subsequently. 

Lemma 3.1: Consider the system and the following 
observer: 

xi = V — /c 7 ryXi,Xi( 0 ) = x? G R" and k > 0 ( 6 ) 

Assume that a = 0, x is bounded and never crosses zero, so 
that the output y is always well defined. Let xi = x — xi G 
R" denote the estimation eiTor. If v(t) G R" is bounded 
and such that the measured direction y(t) is persistently 
exciting, then the equilibrium xi = 0 is Uniformly Globally 
Exponentially Stable (UGES). 

Proof: Differentiating x and using Q and (|^ one gets 
the following linear time varying system: 

Xl = fcTTyX = —klTyXl, 
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Using the assumption of persistent excitation characterized 
by relation ([^ a direct application of [20, Lemma 5] proves 
that Xi is UGES. More explicitly, one verifies that the 
transition matrix $ associated with the above system satisfies 

< ||$(f,r)|| < exp-'>'(*-^), (7) 

where 7 = s(i+k-^SY ' " 

The interest of this result lies in the extreme simplicity of 
the observer design and, more importantly, in the property 
of global stability and explicit bounds on the convergence 
rate of the observer. 

Lemma 3.2: Consider the system ([T]|^ and the above filter 
([g. If 

• V G ffi" is bounded and such that the measured direction 
y is persistently exciting, 

• a: is bounded and never crosses zero, and 

• a € K" is constant 

then |a;| (and hence \xi\) is uniformly bounded w.r.t. initial 
conditions and ultimately bounded by ^|o|. 

Proof: It is straightforward to verify that, in this case, 
the error-system equation is; 

Xi = a — kiTyXi, (8) 

whose general solution is: 

ii(f) = <I>(f, O)ii(O) + f $(T,f)adr 

Jo 

Using 0 it follows that |ii(f)| < (|i°| + i|a|) and 
limt_>+oo |5i(f)| < Since x is bounded by definition, 
it follows that is also bounded. ■ 

For the design of an exponentially stable observer in the 
case where a f 0 the following two technical lemmas are 
instrumental. 

Lemma 3.3: Assume that y G is persistently excit¬ 

ing. The matrix-valued function M(t) solution to; 

M = I - kiTyM, M(0) = M{0f = M° >0 (9) 


is bounded and always invertible, and its condition number 
is bounded. 

Proof: See appendix [B] ■ 

Lemma 3.4: Assume that y G S'^~^ is persistently excit¬ 
ing and y is uniformly continous. The dual output y* := 
is also persistently exciting. 

Proof: See appendix [C] ■ 

The observer design presented hereafter is based on the 
association of the filter 0 that ensures, as we will show, 
that the variable z := xi + Ma converges to x, with a 
second filter that provides an estimate z* of z* := M~^z. It 
then suffices to pre-multiply this second estimate by M to 
obtain an estimate of x. The following theorem specifies the 
observer design and its convergence properties in the case 
where the output y is persistently exciting. 

Theorem 3.5: Consider the system 00 along with (|^ 
and (|^. Define the virtual observer z as follows 


z := xi + Ma 


( 10 ) 


and the dual observer z* of z* := M as follows 


z* = v* -k*Try,z*, z*( 0 ) = zj ( 11 ) 

with V* := M~^ (y — M~^Xi) a known term and k* any 
positive gain. If y is persistently exciting in the sense of 
Lemma 2.2 then the virtual error := x — z, the position 
error x = x — Mx*, and the adaptation error a — a (with 
d := z* — M~^xi), globally exponentially converge to zero. 

Proof: The proof proceeds step by step. Concerning the 
convergence of Xz to zero, one easily verifies, using an, 
that: 

z = V + a-\-kiTyZ (12) 

Differentiating Xz, and using ([^ and ( fT^ , one obtains: 

Xz = k-KyZ = —kTTyXz 


This equation being the same as the one for xi in the case 
where a = 0, one concludes as in Lemma |3.1| that Xz = 0 
is uniformly globally exponentially stable, provided that y is 
persistently exciting. 

Concerning the convergence of x to zero, differentiating 
z* ;= M~^z, and using ([^ and (|^, yields; 

z* = V* (13) 

Now, differentiating z* = z* — z*, and using ( [TT] i and ( [T3] l, 
it comes that: 


Z* = k*TTy*X* 

= —k*Try* (z* — z*) 

Since z* = M~^z, Xz = x — z, and TTy*M~^x = 0, one 
easily verifies that TTy*z* = —iTy*M~^Xz. Therefore: 

Z* = —k*'Ky*Z* — k*'Ky*M~^Xz 

Since y* is a persistently exciting (from Lemma |3.4| i, the 
above equation is similar to the one of xi in the case 
where a = 0 , except for the additive ’’perturbation” term 
—k*'Ky*M~^Xz which converges exponentially to zero, due 
to the exponential convergence of Xz to zero. It is immediate 
to show that this exponentially vanishing perturbation does 
not prevent z* from globally converging to zero exponen¬ 
tially. Since x = Mz* + Xz, and since z* and Xz globally 
converge to zero exponentially, the position error x also 
globally converge exponentially to zero. 

Finally, using the definition of a, one gets a — a = z* 
whose exponential convergence to zero has already been 
established. ■ 


IV. Simulation 

We consider the example of a moving target point ob¬ 
served by a camera. The point moves in the 3D space (n = 3) 
along a circular trajectory at a fixed altitude (z = 3m) 
above the ground. The frame associated to the camera is 
located at the origin of the inertial frame whose optical 
axis is aligned with the z-axis and looks up at the moving 
point. The measure y G corresponds to the spherical 
projection of the point, given by the algebraic transformation 
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y = |^p-|, where is the projective measure provided by 
the camera. The measurement of the velocity v is biased 
by a = (0.33, 0.66, 0.99)^, and v is chosen so that 
v + a = (—0.5sin0.5f, 0.5cos0.5f, 0)^. The following 
values of the observer gains are used: k = 0.5 and k* = 5. 


in the case where the 3D bearing measurement y is calculated 
from the position x to which a uniform noise w taking values 
in the interval [—0.5m, 0.5m] is added. Figures (4]|6i show 


that the high frequency part of the noise is filtered by the 
proposed algorithm so that the performance of the proposed 
observer is not much reduced. 




Fig. 1: Evolution of the system/observer pair in 3D space 

Fig. 4: Evolution of the system/observer pair when the output 
is affected by a noise 



Eig. 2: Evolution of the observer error with respect to time 



Fig. 3: Evolution of the estimate a with respect to time 

In Eigures (jT]|^ the performance of the observer in the 
ideal noise-free case is shown. Erom these figures one can 
observe the exponential convergence of all estimation errors 
to zero. In figures the observer algorithm is simulated 



t 

Eig. 5: Evolution of the observer error in the presence of 
measurement noise 



Eig. 6: Evolution of the estimate a in the presence of 
measurement noise 
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V. Concluding remarks 

In this paper, we discussed the issue of observability of a 
moving object in M" from bearing measurement, proposed a 
nonlinear observer of the object’s position in the case where 
the measured velocity of the object is biased, and carried 
out a detailed analysis of this observer in the case where 
the bearing measurement satisfies a condition of persistent 
excitation. There is an increasing number of emerging ap¬ 
plications that can make use of such an observer. We think, 
in particular, of applications involving cameras for relative 
localization of mobile robot teams. We believe that extending 
the observer design methodology described in the paper to 
the estimation of the relative pose between to mobile objects 
evolving in SE{n), with applications in SE{3), is possible. 
This is one of future extensions of this work. 
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Appendix 

A. Proof of lemma |2.2| 

Let us hrst show that 0 implies Q. 

For r G [t,t + 6] one has 

/ yi{si) \ 

\b^yiT)\^ = \b^iyit) + ; (t - 

\ yniSn) j 

> — 2nd{6){T — t) — 4n^d(i5)^(r — 

for some si G [t,T] (i = l,...,n) and d{6) = 

sup^gfj |y(r)|. Choose b = y{t) so that = 1, 

then 


the existence of an interval [^ 1 ,^ 2 ] C [t,t + 5] such that 
(f 2 - h) = ei(e) > 0 and 1.5 > \y{t 2 ) - y{ti)\ > 
^ 2 (e) := ^ei(e) > 0. Now, = cos( 6 »(f, 6 ))^ 

with 0{t,b) denoting the angle between b and y{t). The 
previous inequality in turn implies that cos( 0 (fi, 6 ))^ 
and cos{9{t2,b))^ cannot be both equal to one. Therefore, 
cos( 0 (fi, 5))^ = l — ui{e,b) and cos( 6 *(f 2 , = 1 —^ 22 ( 6 ,5), 

with max{i'i, 02 ) > b) > 0. Since v{e ,.) is a continuous 
function depending on variables that take values in the 
compact set 5””“^ it reaches its bounds. This implies that 
v{e,b) > v{e) > 0, V 6 G 5'”“^. Using the fact that the 
uniform boundedness of y yields the uniform boundedness 
of 9 and thus of cos(0)^, this in turn implies that 
V 6 G 

[ l^^?/('r)PdT= / cos( 6 »(T, 6 )) 2 dr < (f2-fi)(l-e3(e)) 

Jti Jti 

with € 3 ( 6 ) > 0. Therefore, V 6 G 5'”“^: 

|6'^2/(r)|2dr = //' |6'^y(r)|2dr + \b'^y{T)\‘^dT 

+ It^^ l&'^yMpdr 

< (fi - f) + (f2 - fi)(l - e3(e)) 

+ (f + 5 — ^ 2 ) 

< S- /r(e) 

with /r(e) = ei(e)e 3 (e) > 0 . 

B. Proof of Lemma |d.3| 

To prove that M is bounded, is suffices to ensure that, 
for any constant vector b G |M 6 | is bounded. Define 

u = Mb, it follows that: 

u = b — kiTyU 

This equation is similar to the equation ([^ of ii. Therefore 

\M{t)b\ < \M{0)b\ -f ^ , V 6 G 
7 


> 1 — 2nd{S){T — t) — 4n‘^d{S)^(T — 


and 


/•t+S 4 

/ |5^2/(r)pdT > 6 — nd{5)8^ — -n^d{S)^S' 

Jt 3 

Clearly there exists e > 0 (independent of t) such that 

pt-\-5 

d{T) < e ^ J \b’^yiT)\‘^dT > S — y 

Let us proceed by contradiction and assume that 0 does 
not hold, i.e. \y{T)\ < e, Vf G [t,t + <5], then d{S) < e and 
\b"’"y{T)\^dT > S — y. This contradicts 0 according to 
which 


2r3 


\TTy(^^)b\^dT = 5-j \b^y{T)\^dT > y 

Therefore 0 holds true. 


rt-\-5 


We now show that 0 implies Q 

Using the (assumed) uniform continuity of y, 0 implies 


To show that M is an invertible matrix, dehne A := det(M). 
From Jacobi’s formula, one has 

A = Atr(M“^M) 

= Atr(M“^ — kM~^TYyM) 

= Atr(M“^) — kAir{TryMM~^) 

=-fc(n-1)A +Ati-(M-i) (14) 

Note that this equation holds even if M is not invert¬ 
ible. Indeed, using the fact that det(M) = nr=i 
tr(M“^) = 7“’ "'dh \i (i = 1.. .n) the eigenvalues 

of M, one verifies that 

n n 

A = —k{n — 1)A + E n A, (15) 

i=l 

Since M(0) is symmetric positive definite by assumption, 
all eigenvalues of M(0) are positive and A(0) > 0. Assume 
that A is equal to zero for the first time at the time instant 
to > 0. Then, tr(M(f)) > 0 on [0,fo) and tr(M(f)) > 0. 
In view of ([HI), A with r{t) the solution to the 
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equation f = —k{n — l)r, with r(0) = A(0). Therefore 
A(f) > A(0) exp(—fc(n — l)f) > 0, Vf. This contradicts the 
existence of to and proves that M(t) is always invertible. 
Let us now prove that A(f) is lower bounded by a positive 
number. Rewrite equation ( [l4| ) as follows 

A = - (tr(M-i) - (n-l)fc) A, (16) 

Using the fact that tr(M“^) > „ , this equation shows 

that A > 0 if A < Therefore A is ultimately 

lower bounded by 

Finally, since A is lower bounded by a positive number 
and M is upper bounded, it follows (by direct application of 
[21]) that the condition number 

is upper bounded. 


C. Proof of Lemma \3.4\ 

From the equation (|^ of M one gets 

^{My*) = y* + My* 
at 

Therefore 

(Mj/*)(t) = iMy*m + f\y* + My*)(r)dr 
Jo 

and 


\{My*m\ > 


{y* + My*)(T) 


\iMy*m\ 


Define: 

. kM '■= ||M(f)||, which implies that 

\{My*){t)\ < kM, Vf, 

. c(t) := sup^g[o.t] 

. w{t) := {y* + My*){t). 

( Viisi) 


One has w{t) = y*{0) + 


T + {My*){T) for 


\ VlYn) , 

some Si G [0, r] (i = 1,..., n). Therefore 





( ytisi) ^ 


w(r)dr| > 

/o2/*(0)dr|- 

lo 

^ VtiSn) ) 

rdr 


-l/o(^y*)MdT| 
> /(tAi) ; U >i 


with := t — nc{ti)Y — kMc{ti)t. The function 

f{.,ti) monotically increases for 0 < t < t 2 = 

if c{ti) < Define g{c) := t*(c) — - kMct*(c) 

with t*{c) = The function g decreases monotically 

on [0, jA] Yvith g(o) = +00 and g{l/kM) = 0. Let 
c > 0 denote the value of c such that g{c) = 2kM, and 
set ti = t 2 = c(fi) = sup^g[o,t^] |2/*('r)l < 

c then |/g^iu(T)dr| > 2kM- Since \{My*)(ti)\ > 

|/o^u;(r)dT| - |(Mj/*)(0)| with |(My*)(0)| < kn, one 


concludes that \(My*){ti)\ > Lm (contradiction). Therefore, 
sup^g[g |t)*(T)| > c, which proves the existence of a time- 
instant to G [0,fi] such that \y*{t 3 )\ > c. 

The same proof repeated on every interval [kti, {k + l)fi] 
{k G N) shows that |y*| is periodically larger than c > 0. 
This establishes that y* is persistently exciting. 
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